#include "ocu.h"

float LIMIT(float in, float min, float max) {
  if (in > max)
    return max;
  if (in < min)
    return min;
  return in;
}

int generateJudgment(String str)//判断Wifi名称是否合法
{
  const char *p = str.c_str();
	int len = strlen(p);
  //Serial.println(str+" "+len);
  if(len==0)return 1;
	int i = 0;
	for(i = 0;i < len;i++){
		if(*(p+i) & 0x80||str.equals("  ")|| len<3||str.equals("A Hotel")){
			return 1;
		}else{
		
		}
	}
	return 0;
}

void converV_b_to_n_RT(float RT[3][3], float yaw, float xb, float yb, float zb, float *xn, float *yn, float *zn)
{
    float x, y, z;
    x = RT[0][0] * xb + RT[0][1] * yb + RT[0][2] * zb;
    y = RT[1][0] * xb + RT[1][1] * yb + RT[1][2] * zb;
    z = RT[2][0] * xb + RT[2][1] * yb + RT[2][2] * zb;

    *xn = x * cosd(yaw) - y * sind(yaw);
    *yn = x * sind(yaw) + y * cosd(yaw);
    *zn = z;
}


void converV_b_to_leg(char leg, float xb, float yb, float zb, float *xl, float *yl, float *zl)
{
    *xl = xb - vmc_all.H / 2 * vmc[leg].flag_fb;
    *yl = yb - vmc_all.W / 2 * vmc[leg].flag_rl;
    *zl = zb;
}

void converV_n_to_b_w_yaw(float yaw, float xn, float yn, float zn, float *xb, float *yb, float *zb)
{
    float x, y, z;

    *xb = xn * cosd(yaw) - yn * sind(yaw);
    *yb = xn * sind(yaw) + yn * cosd(yaw);
    *zb = zn;
}



void DigitalLPF(float in, float* out, float cutoff_freq, float dt) {
    float input_reg=in;
    if (cutoff_freq <= 0.0f || dt <= 0.0f) {
        *out = input_reg;
    }
    float lpf_cof;
    int frep=cutoff_freq;
    
    if(cutoff_freq<1)
            lpf_cof=1/(2*F_PI*cutoff_freq);
    else if(cutoff_freq>=1&&cutoff_freq<5)
      lpf_cof=LPF_COF_1t5Hz;
    else if(cutoff_freq>=5&&cutoff_freq<10)
      lpf_cof=LPF_COF_5t10Hz; 
    else if(cutoff_freq>=10&&cutoff_freq<15)
      lpf_cof=LPF_COF_10t15Hz;
    else if(cutoff_freq>=15&&cutoff_freq<20)
      lpf_cof=LPF_COF_15t20Hz;
    else if(cutoff_freq>=20&&cutoff_freq<25)
      lpf_cof=LPF_COF_20t25Hz;
    else if(cutoff_freq>=25&&cutoff_freq<30)
      lpf_cof=LPF_COF_25t30Hz;
    else if(cutoff_freq>=30&&cutoff_freq<50)
      lpf_cof=LPF_COF_30t50Hz;
    else if(cutoff_freq>=50&&cutoff_freq<70)
      lpf_cof=LPF_COF_50t70Hz;
    else if(cutoff_freq>=70&&cutoff_freq<100)
      lpf_cof=LPF_COF_70t100Hz;   
    else
            lpf_cof=1.0f/(2*F_PI*cutoff_freq);
    float rc = lpf_cof;
    float alpha = LIMIT(dt/(dt+rc), 0.0f, 1.0f);
    *out += (input_reg - *out) * alpha;
}
